#ifndef LPSOLVER_HPP
#define LPSOLVER_HPP

#include <Eigen/Sparse>
#include <Eigen/Dense>
//****************************************************************************//
// TODO Why do I have to add this again they were any way added in
// <Eigen/PaStiXSupport>, but are not considered for some reason, without these
// there are compiler errors
#include <complex>
extern "C" {
#include <pastix_nompi.h>
#include <pastix.h>
}

#define COMPLEX std::complex<float>
#define DCOMPLEX std::complex<double>
//****************************************************************************//
#include <Eigen/PaStiXSupport>

#include <cmath>
#include <vector>
#include <algorithm>

#include <iostream>

// FIXME Remove me
using std::cout;
using std::endl;
// FIXME Remove me

// FIXME rule of four and half for all classes
// http://stackoverflow.com/questions/3279543/what-is-the-copy-and-swap-idiom
// FIXME Move methods to non members of class if you can read scott
namespace green {

namespace internal {
// TODO Move it to diffeent file or different namespace like in constants header
// for easy modifications
const double delta = 0.3;
const double gamma = 0.1;

/*!
 * \brief The Problem has all three required inputs to solve LP problem
 * A(Constraints, mXn), b(RHS, mX1), c(Coefficients, nX1)
 *
 * TODO May be I have to take sizes of matrices at runtime as input parameters
 * via constructor, as they are fixed through out algorithm hmm...
 */
class Problem {
 public:
  Problem(int rows, int cols) : A(rows, cols), b(rows), c(cols) {}

  Eigen::SparseMatrix<double> A;
  Eigen::VectorXd b;
  Eigen::VectorXd c;
};

/*!
 * \brief The Omega class means Small Omega class that contains point (x, y, s,
 * tau, kappa) in
 * primal-dual space
 * Its a container, it does not do anything by itself.
 *
 */
class Omega {
 public:
  Eigen::VectorXd x;
  Eigen::VectorXd y;
  Eigen::VectorXd s;
  double tau;
  double kappa;

  /*!
   * \brief operator +=
   * \param other
   * \return
   */
  Omega& operator+=(const Omega& other) {
    x += other.x;
    y += other.y;
    s += other.s;
    tau += other.tau;
    kappa += other.kappa;

    return *this;
  }

  /*!
   * \brief operator +
   * \param other
   * \return
   */
  const Omega operator+(const Omega& other) const {
    Omega result = *this;
    result += other;

    return result;
  }

  /*!
   * \brief operator *=
   * \param value
   * \return
   */
  Omega& operator*=(const double& value) {
    x *= value;
    y *= value;
    s *= value;
    tau *= value;
    kappa *= value;

    return *this;
  }

  /*!
   * \brief getInitialPoint
   * \param b
   * \param c
   * \return
   *
   * Initial point omega is calculated such that omega > 0. Based on Homogeneous
   * and self-dual matlab code
   */
  Omega static getInitialPoint(const internal::Problem& problem) {
    internal::Omega initialPoint;

    initialPoint.x = Eigen::VectorXd::Ones(problem.c.rows());
    initialPoint.y = Eigen::VectorXd::Zero(problem.b.rows());
    initialPoint.s = Eigen::VectorXd::Ones(problem.c.rows());
    initialPoint.tau = 1;
    initialPoint.kappa = 1;

    return initialPoint;
  }
};

/*!
 * \brief operator *
 * \param value
 * \param other
 * \return
 */
const Omega operator*(const double& value, const Omega& other) {
  Omega result = other;
  result *= value;

  return result;
}

/*!
 * \brief operator *
 * \param other
 * \param value
 * \return
 */
const Omega operator*(const Omega& other, const double& value) {
  return value * other;
}

/*!
 * \brief operator <<
 * \param stream
 * \param omega
 * \return
 */
std::ostream& operator<<(std::ostream& stream, const Omega& omega) {
  stream << std::endl;
  stream << "###################################" << std::endl;
  stream << "Value of x: " << omega.x << std::endl;
  stream << "Value of y: " << omega.y << std::endl;
  stream << "Value of s: " << omega.s << std::endl;
  stream << "Value of tau: " << omega.tau << std::endl;
  stream << "Value of kappa: " << omega.kappa << std::endl;
  stream << "###################################" << std::endl;

  return stream;
}

/*!
 * \brief The Residuals class
 */
class Residuals {
 public:
  Residuals(const internal::Problem& problem, const internal::Omega& omega)
      : primal(getPrimal(problem, omega)),
        dual(getDual(problem, omega)),
        primalDualGap(getPrimalDualGap(problem, omega)) {}
  const Eigen::VectorXd primal;
  const Eigen::VectorXd dual;
  const double primalDualGap;

 private:
  Eigen::VectorXd getPrimal(const internal::Problem& problem,
                            const internal::Omega& omega) const {
    return ((problem.b * omega.tau) - (problem.A * omega.x));
  }

  Eigen::VectorXd getDual(const internal::Problem& problem,
                          const internal::Omega& omega) const {
    return ((problem.c * omega.tau) - (problem.A.transpose() * omega.y) -
            omega.s);
  }

  double getPrimalDualGap(const internal::Problem& problem,
                          const internal::Omega& omega) const {
    return (omega.kappa + (problem.c.transpose() * omega.x) -
            (problem.b.transpose() * omega.y));
  }
};

/*!
 * \brief The ResidualHat class
 */
class ResidualHat {
 public:
  /*!
   * \brief ResidualHat
   * \param residuals
   * \param currentPoint
   * Used for predictor direction residuals
   */
  ResidualHat(const internal::Residuals& residuals,
              const internal::Omega& currentPoint)
      : primal(residuals.primal),
        dual(residuals.dual),
        primalDualGap(residuals.primalDualGap),
        XS(-1 * (currentPoint.x.array() * currentPoint.s.array()).matrix()),
        tauKappa(-1 * currentPoint.tau * currentPoint.kappa) {}

  /*!
   * \brief ResidualHat
   * \param residuals
   * \param currentPoint
   * Used for corrector direction residuals
   */
  ResidualHat(const internal::Residuals& residuals,
              const internal::Omega& currentPoint,
              const internal::Omega& predictorDirection,
              const double& barrierParameter, const double& eta,
              const double& gamma)
      : primal(eta * residuals.primal),
        dual(eta * residuals.dual),
        primalDualGap(eta * residuals.primalDualGap),
        XS(getXS(currentPoint, predictorDirection, barrierParameter, gamma)),
        tauKappa(getTauKappa(currentPoint, predictorDirection, barrierParameter,
                             gamma)) {}

  const Eigen::VectorXd primal;
  const Eigen::VectorXd dual;
  const double primalDualGap;
  const Eigen::VectorXd XS;
  const double tauKappa;

 private:
  Eigen::VectorXd getXS(const internal::Omega& currentPoint,
                        const internal::Omega& predictorDirection,
                        const double& barrierParamter, const double& gamma) {
    return (-1 * (currentPoint.x.array() * currentPoint.s.array()).matrix()) +
           (gamma * barrierParamter *
            Eigen::VectorXd::Ones(currentPoint.x.rows())) -
           ((predictorDirection.x.array() * predictorDirection.s.array())
                .matrix());
  }

  double getTauKappa(const internal::Omega& currentPoint,
                     const internal::Omega& predictorDirection,
                     const double& barrierParamter, const double& gamma) {
    return (-1 * currentPoint.tau * currentPoint.kappa) +
           (gamma * barrierParamter) -
           (predictorDirection.tau * predictorDirection.kappa);
  }
};

/*!
 * \brief The StepSize class
 */
class StepSize {
 public:
  StepSize(const internal::Omega& currentPoint,
           const internal::Omega& directionPoint)
      : alphaPrimal(getAlphaPrimal(currentPoint, directionPoint)),
        alphaDual(getAlphaDual(currentPoint, directionPoint)),
        alphaTauFinal(std::min(alphaPrimal, alphaDual)),
        alphaKappaFinal(std::max(alphaPrimal, alphaDual)) {}

  StepSize(const StepSize& stepSize, const double& delta)
      : alphaPrimal(std::min((stepSize.alphaPrimal + delta), 1.0)),
        alphaDual(std::min((stepSize.alphaDual + delta), 1.0)),
        alphaTauFinal(std::min((stepSize.alphaTauFinal + delta), 1.0)),
        alphaKappaFinal(std::min((stepSize.alphaKappaFinal + delta), 1.0)) {}

  const double alphaPrimal;
  const double alphaDual;
  const double alphaTauFinal;
  const double alphaKappaFinal;

 private:
  constexpr static double alphaMultiplier{0.99995};

  /*!
   * \brief getAlphaPrimal
   * \param currentPoint
   * \param directionPoint
   * \return
   */
  double getAlphaPrimal(const internal::Omega& currentPoint,
                        const internal::Omega& directionPoint) const {
    return getAlphaPrimalOrDual(currentPoint.x, directionPoint.x,
                                currentPoint.tau, directionPoint.tau,
                                currentPoint.kappa, directionPoint.kappa);
  }

  /*!
   * \brief getAlphaDual
   * \param currentPoint
   * \param directionPoint
   * \return
   */
  double getAlphaDual(const internal::Omega& currentPoint,
                      const internal::Omega& directionPoint) const {
    return getAlphaPrimalOrDual(currentPoint.s, directionPoint.s,
                                currentPoint.tau, directionPoint.tau,
                                currentPoint.kappa, directionPoint.kappa);
  }

  /*!
   * \brief getAlphaPrimalOrDual
   * \param currentPrimalOrDual
   * \param directionPrimalOrDual
   * \param currentTau
   * \param directionTau
   * \param currentKappa
   * \param directionKappa
   * \return
   *
   * Tau and Kappa are calculated twice as this functio is call twice for both
   * primal and dual cases.
   * TODO Check for better way or ignore if its only small penalty
   */
  double getAlphaPrimalOrDual(const Eigen::VectorXd& currentPrimalOrDual,
                              const Eigen::VectorXd& directionPrimalOrDual,
                              const double& currentTau,
                              const double& directionTau,
                              const double& currentKappa,
                              const double& directionKappa) const {
    double alpha = (currentPrimalOrDual.array() /
                    directionPrimalOrDual.array().abs()).minCoeff();

    double alphaTau = currentTau / std::abs(directionTau);
    double alphaKappa = currentKappa / std::abs(directionKappa);

    return alphaMultiplier * std::min(std::min(alpha, alphaTau), alphaKappa);
  }
};

/*!
 * \brief operator <
 * \param stepSize1
 * \param stepSize2
 * \return
 */
bool operator<(const StepSize& stepSize1, const StepSize& stepSize2) {
  return (stepSize1.alphaPrimal * stepSize1.alphaDual) <
         (stepSize2.alphaPrimal * stepSize2.alphaDual);
}

/*!
 * \brief operator >
 * \param stepSize1
 * \param stepSize2
 * \return
 */
bool operator>(const StepSize& stepSize1, const StepSize& stepSize2) {
  return !(stepSize1 < stepSize2);
}

/*!
 * \brief operator <=
 * \param stepSize1
 * \param stepSize2
 * \return
 */
bool operator<=(const StepSize& stepSize1, const StepSize& stepSize2) {
  return (stepSize1.alphaPrimal * stepSize1.alphaDual) <=
         (stepSize2.alphaPrimal * stepSize2.alphaDual);
}

/*!
 * \brief operator >=
 * \param stepSize1
 * \param stepSize2
 * \return
 */
bool operator>=(const StepSize& stepSize1, const StepSize& stepSize2) {
  return (stepSize1.alphaPrimal * stepSize1.alphaDual) >=
         (stepSize2.alphaPrimal * stepSize2.alphaDual);
}

/*!
 * \brief operator *
 * \param stepSize
 * \param omega
 * \return
 */
const Omega operator*(const StepSize& stepSize, const Omega& omega) {
  Omega result;

  result.x = stepSize.alphaPrimal * omega.x;
  result.s = stepSize.alphaDual * omega.s;
  result.y = stepSize.alphaDual * omega.y;
  result.tau = stepSize.alphaTauFinal * omega.tau;
  result.kappa = stepSize.alphaKappaFinal * omega.kappa;

  return result;
}

/*!
 * \brief operator *
 * \param omega
 * \param stepSize
 * \return
 */
const Omega operator*(const Omega& omega, const StepSize& stepSize) {
  return stepSize * omega;
}

/*!
 * \brief operator <<
 * \param stream
 * \param stepSize
 * \return
 */
std::ostream& operator<<(std::ostream& stream, const StepSize& stepSize) {
  stream << std::endl;
  stream << "###################################" << std::endl;
  stream << "Value of Alpha Primal: " << stepSize.alphaPrimal << std::endl;
  stream << "Value of Alpha Dual: " << stepSize.alphaDual << std::endl;
  stream << "Value of Alpha Tau: " << stepSize.alphaTauFinal << std::endl;
  stream << "Value of Alpha kappa: " << stepSize.alphaKappaFinal << std::endl;
  stream << "###################################" << std::endl;

  return stream;
}
/*!
 * \brief The SolverState enum
 */
enum class SolverState {
  FEASIBLE, INFEASIBLE, IN_PROGRESS
};

/*!
 * \brief The NewtonDirection class
 */
class NewtonDirection {

 public:
  NewtonDirection(const internal::Problem& problem,
                  const internal::Omega& currentPoint)
      : problem(problem),
        currentPoint(currentPoint),
        DInverse(getDInverse(currentPoint)),
        onesDn(Eigen::VectorXd::Ones(currentPoint.s.rows())),
        SInverse((onesDn.array() / currentPoint.s.array()).matrix()),
        XInverse((onesDn.array() / currentPoint.x.array()).matrix()) {
    // Factorize
    directSolver.compute(getNormalEquationLHS(problem));

    if (directSolver.info() != Eigen::Success) {
      // TODO Exception or what?
      cout << "Factorization failed!" << endl;
    }
    // TODO I dont feel good about this method, I would like to keep all memeber
    // variables constant and set them before constructor is invoked,
    // but can't call compute method before constructor as it is not created yet
    Eigen::VectorXd gamma1RHS = problem.b + (problem.A * DInverse * problem.c);
    gamma2 = directSolver.solve(gamma1RHS);

    gamma1 = DInverse * ((problem.A.transpose() * gamma2) - problem.c);

    directionTauDenominator = ((1 / currentPoint.tau) * currentPoint.kappa +
                               getXITransposePSI(gamma1, gamma2));

  }

  /*!
   * \brief getDirection
   * \param residualHat
   * \return
   */
  internal::Omega getDirection(const internal::ResidualHat& residualHat) const {
    internal::Omega direction;

    Eigen::VectorXd psiSubRHS = residualHat.dual - XInverse * residualHat.XS;

    Eigen::VectorXd psi2RHS =
        residualHat.primal + ((problem.A * DInverse) * psiSubRHS);
    Eigen::VectorXd psi2 = directSolver.solve(psi2RHS);

    Eigen::VectorXd psi1 =
        DInverse * ((problem.A.transpose() * psi2) - psiSubRHS);

    direction.tau = (residualHat.primalDualGap +
                     ((1 / currentPoint.tau) * residualHat.tauKappa) -
                     getXITransposePSI(psi1, psi2)) /
                    directionTauDenominator;

    direction.x = psi1 + gamma1 * direction.tau;
    direction.y = psi2 + gamma2 * direction.tau;

    direction.s = XInverse * residualHat.XS - DInverse * direction.x;

    direction.kappa =
        (1 / currentPoint.tau) *
        (residualHat.tauKappa - currentPoint.kappa * direction.tau);

    return direction;
  }

  /*!
   * \brief getPredictorDirection
   * \param residuals
   * \return
   */
  internal::Omega getPredictorDirection(internal::Residuals& residuals) const {

    internal::ResidualHat residualHat(residuals, currentPoint);

    return getDirection(residualHat);
  }

  /*!
   * \brief getCorrectorDirection
   * \param predictorDirection
   * \param barrierParameter
   * \return
   */
  internal::Omega getCorrectorDirection(
      const internal::StepSize& currentStepSize,
      const internal::Omega predictorDirection,
      const internal::Residuals residuals,
      const double barrierParameter) const {
    double gamma =
        getGamma(currentStepSize, predictorDirection, barrierParameter);
    double eta = getEta(gamma);

    internal::ResidualHat residualHat(residuals, currentPoint,
                                      predictorDirection, barrierParameter, eta,
                                      gamma);

    return getDirection(residualHat);
  }

  /*!
   * \brief getCentralityDirection
   * \param currentPoint
   * \param centralityTargetT
   * \param centralityTargetU
   * \return
   */
  internal::Omega getCentralityDirection(
      const internal::Omega& currentPoint,
      const Eigen::VectorXd& centralityTargetT,
      const double centralityTargetU) const {
    internal::Omega centralityDirection;

    Eigen::VectorXd psi2RHS = -1 * (problem.A * SInverse * centralityTargetT);
    Eigen::VectorXd psi2 = directSolver.solve(psi2RHS);

    Eigen::VectorXd psi1 = DInverse * ((XInverse * centralityTargetT) +
                                       (problem.A.transpose() * psi2));

    centralityDirection.tau = ((1 / currentPoint.tau) * centralityTargetU +
                               getXITransposePSI(psi1, psi2)) /
                              directionTauDenominator;

    centralityDirection.x = psi1 - gamma1 * centralityDirection.tau;
    centralityDirection.y = psi2 - gamma2 * centralityDirection.tau;

    const Eigen::DiagonalMatrix<double, Eigen::Dynamic> S(currentPoint.s);
    centralityDirection.s =
        XInverse * (centralityTargetT - (S * centralityDirection.x));

    centralityDirection.kappa =
        ((1 / currentPoint.tau) *
         (centralityTargetU - currentPoint.kappa * centralityDirection.tau));

    return centralityDirection;
  }

 private:
  Eigen::PastixLLT<Eigen::SparseMatrix<double>, Eigen::Lower> directSolver;

  const internal::Problem& problem;
  const internal::Omega& currentPoint;
  // These variables are not changed in iteration for different directions, so
  // they are stored
  const Eigen::DiagonalMatrix<double, Eigen::Dynamic> DInverse;
  // Ones with dimension n
  const Eigen::VectorXd onesDn;
  const Eigen::DiagonalMatrix<double, Eigen::Dynamic> SInverse;
  const Eigen::DiagonalMatrix<double, Eigen::Dynamic> XInverse;

  Eigen::VectorXd gamma1;
  Eigen::VectorXd gamma2;
  double directionTauDenominator;

  /*!
   * \brief getDInverse
   * \param currentPoint
   * \return
   */
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> getDInverse(
      const internal::Omega& currentPoint) const {
    Eigen::VectorXd sInverseX = currentPoint.x.array() / currentPoint.s.array();
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> DInverse(sInverseX);

    return DInverse;
  }

  /*!
   * \brief getNormalEquationLHS
   * \param probleml
   * \param currentPoint
   * \return A*D.Inverse()*A.transpose()
   * D = X.inverse() * S, DInverse = S.inverse() * X = diagonalMatrix( x./s ),
   * x./s is coefficient wise division
   */
  Eigen::SparseMatrix<double> getNormalEquationLHS(
      const internal::Problem& problem) const {
    return problem.A * DInverse * problem.A.transpose();
  }

  /*!
   * \brief getXITransposePSI
   * \return
   *
   * xi1.transpose * psi1 - xi2.transpose * psi2;
   * Above expression is equivalent to c.transpose() * psi1 - b.transpose() *
  * psi2
   */
  double getXITransposePSI(const Eigen::VectorXd& psi1,
                           const Eigen::VectorXd& psi2) const {
    //(problem.c.transpose() * psi1) - (problem.b.transpose() * psi2));
    // For some reason above expression is failing, so the following workaround
    double XITransposePSI = (problem.c.transpose() * psi1);
    XITransposePSI = (problem.b.transpose() * psi2) - XITransposePSI;
    return XITransposePSI;
  }

  /*!
   * \brief getBarrierParameterAfterPredictor
   * \param currentPoint
   * \param currentStepSize
   * \param predictorDirection
   * \return
   */
  double getBarrierParameterAfterPredictor(
      const internal::StepSize& currentStepSize,
      const internal::Omega& predictorDirection) const {
    internal::Omega predictorWithAlpha = currentStepSize * predictorDirection;

    return (((currentPoint.x + predictorWithAlpha.x).transpose() *
             (currentPoint.s + predictorWithAlpha.s)) +
            (currentPoint.tau + predictorWithAlpha.tau) *
                (currentPoint.kappa + predictorWithAlpha.kappa)) /
           (currentPoint.x.rows() + 1);
  }

  /*!
   * \brief getGamma
   * \param currentPoint
   * \param currentStepSize
   * \param predictorDirection
   * \param barrierParamter
   * \return
   */
  double getGamma(const internal::StepSize& currentStepSize,
                  const internal::Omega& predictorDirection,
                  const double& barrierParamter) const {
    double barrierParamterAfterPredictor =
        getBarrierParameterAfterPredictor(currentStepSize, predictorDirection);

    double barrierParamterRatio =
        barrierParamterAfterPredictor / barrierParamter;

    if (barrierParamterRatio <= 1e-2) {
      return std::pow((barrierParamterRatio), 2);
    } else {
      return std::min(1e-1,
                      std::max(std::pow((barrierParamterRatio), 3), 1e-4));
    }
  }

  /*!
   * \brief getEta
   * \param gamma
   * \return
   */
  double getEta(const double& gamma) const { return 1 - gamma; }
};
}

/*!
* \brief The LPSolver class
* Expects problem to be in standard form
*
* TODO constraint rhs and coefficients are represented using dense vectors check
* again
*/
class LPSolver {

 public:
  LPSolver() {}

  /*!
   * \brief solve
   * \return
   */
  bool solve(const internal::Problem& problem) {

    internal::SolverState solverState;
    internal::Omega initialPoint = internal::Omega::getInitialPoint(problem);
    // double initialBarrierParameter;
    internal::Omega currentPoint(initialPoint);

    while (true) {
      internal::Residuals residuals(problem, currentPoint);

      solverState =
          getSolverState(problem, currentPoint, initialPoint, residuals);
      // TODO Maybe solver State can be return value of the function which has
      // while loop
      if (solverState != internal::SolverState::IN_PROGRESS) {
        break;
      }

      internal::NewtonDirection newtonDirection(problem, currentPoint);

      auto predictorDirection =
          newtonDirection.getPredictorDirection(residuals);

//      cout << "Tau value: " << predictorDirection.tau << endl;

//      cout << "Predictor Direction: " << predictorDirection << endl;

      internal::StepSize currentStepSize(currentPoint, predictorDirection);

//      cout << "Current StepSize: " << currentStepSize << endl;

      double barrierParameter = getBarrierParameter(currentPoint);

//      cout << "Duality measure: " << barrierParameter << endl;

      auto correctorDirection = newtonDirection.getCorrectorDirection(
          currentStepSize, predictorDirection, residuals, barrierParameter);

//      cout << "Corrector Direction: " << correctorDirection << endl;

      // TODO Testing only mehrotra's predictor corrector algorithm first
      internal::Omega finalDirection = predictorDirection + correctorDirection;

      double temp = finalDirection.x.transpose() * finalDirection.s + finalDirection.tau * finalDirection.kappa;

      cout << "Complementary :" << temp << endl;

      internal::StepSize newStepSize(currentPoint, finalDirection);
      currentPoint = currentPoint + newStepSize * finalDirection;

//      cout << "New Step size: " << newStepSize << endl;
//      cout << "Current solution: " << getFinalSolution(currentPoint) << endl;

//      break;
    }

//    cout << "Final solution: " << getFinalSolution(currentPoint) << endl;

    return true;
  }

 private:

  /*!
   * \brief getFinalSolution
   * \param omega
   * \return
   */
  internal::Omega getFinalSolution(internal::Omega& omega) {
    internal::Omega finalSolution;

    finalSolution.x = omega.x / omega.tau;
    finalSolution.s = omega.s / omega.tau;
    finalSolution.y = omega.y / omega.tau;

    finalSolution.tau = omega.tau;
    finalSolution.kappa = omega.kappa;

    return finalSolution;
  }

  /*!
   * \brief getCentralityTargetU
   * \param trailPoint
   * \param gamma
   * \param barrierParameter
   * \return
   */
  double getCentralityTargetU(const internal::Omega& trailPoint,
                              const double& gamma,
                              const double& barrierParameter) const {
    const double lowerNeighbourhood = gamma * barrierParameter;
    const double upperNeighbourhood = (1 / gamma) * barrierParameter;

    double centralityTarget = trailPoint.tau * trailPoint.kappa;
    if (centralityTarget >= upperNeighbourhood) {
      return centralityTarget = upperNeighbourhood - centralityTarget;
    } else if (centralityTarget <= lowerNeighbourhood) {
      return lowerNeighbourhood - centralityTarget;
    } else {
      return 0;
    }
  }

  /*!
   * \brief getCentralityTargetT
   * \param trailPoint
   * \param gamma
   * \param barrierParameter
   * \return
   */
  Eigen::VectorXd getCentralityTargetT(const internal::Omega& trailPoint,
                                       const double& gamma,
                                       const double& barrierParameter) const {

    Eigen::VectorXd centralityTarget =
        (trailPoint.x.array() * trailPoint.s.array()).matrix();

    const double lowerNeighbourhood = gamma * barrierParameter;
    const double upperNeighbourhood = (1 / gamma) * barrierParameter;

    centralityTarget.unaryExpr(
        [&lowerNeighbourhood, &upperNeighbourhood](const double & element)
            ->double {
      if (element <= lowerNeighbourhood) {
        return lowerNeighbourhood - element;
      } else if (element >= upperNeighbourhood) {
        return upperNeighbourhood - element;
      } else {
        return 0;
      }
    });
    return centralityTarget;
  }

  /*!
   * \brief getTrailPoint
   * \param currentPoint
   * \param currentDirection
   * \param trailStepSize
   * \return
   */
  internal::Omega getTrailPoint(const internal::Omega& currentPoint,
                                const internal::Omega& currentDirection,
                                const internal::StepSize& trailStepSize) const {
    internal::Omega trailPoint;

    trailPoint = currentPoint + (trailStepSize * currentDirection);
    return trailPoint;
  }

  /*!
   * \brief getSolverState
   * \param problem
   * \param omega
   * \return
   */
  internal::SolverState getSolverState(
      const internal::Problem& problem, const internal::Omega& currentPoint,
      const internal::Omega& initialPoint,
      const internal::Residuals& residuals) const {
    // kappa is added in primalDualGap so negate it here
    double primalDualGapRatio =
        std::abs(residuals.primalDualGap - currentPoint.kappa) /
        (currentPoint.tau + std::abs(problem.b.transpose() * currentPoint.y));
    double primalRatio =
        residuals.primal.norm() / (currentPoint.tau + currentPoint.x.norm());
    double dualRatio =
        residuals.dual.norm() / (currentPoint.tau + currentPoint.s.norm());

    // TODO Still barrierParameter ratio is pending, what is initial barrier
    // paramter?
    double tauKappaRatio = (currentPoint.tau / currentPoint.kappa) /
                           (initialPoint.tau / initialPoint.kappa);

    if (primalDualGapRatio < 1e-8 && primalRatio < 1e-8 && dualRatio < 1e-8) {
      return internal::SolverState::FEASIBLE;
    } else if (tauKappaRatio < 1e-8) {
      // FIXME Come here again after computing barrier parameter
      cout << "Problem is infeasible " << endl;
      return internal::SolverState::INFEASIBLE;
    } else {
      return internal::SolverState::IN_PROGRESS;
    }
  }

  /*!
   * \brief getOptimumWeight
   * \param currentPoint
   * \param omega1
   * \param omega2
   * \param currentStepSize
   * \return
   */
  double getOptimumWeight(const internal::Omega& currentPoint,
                          const internal::Omega& omega1,
                          const internal::Omega& omega2,
                          const internal::StepSize& currentStepSize) const {

    using WeightStepSizeType = std::tuple<double, internal::StepSize>;
    // FIXME Move 20 to some constant header
    std::vector<double> weights =
        getUniformlyDistributedPoints(20, currentStepSize);

    std::vector<WeightStepSizeType> weightStepSizeList;

    for (double weight : weights) {
      internal::Omega tempDirectionPoint = omega1 + (weight * omega2);
      internal::StepSize tempStepSize(currentPoint, tempDirectionPoint);

      weightStepSizeList.emplace_back(std::make_tuple(weight, tempStepSize));
    }

    auto optimumWeight =
        std::max_element(weightStepSizeList.begin(), weightStepSizeList.end(),
                         [](const WeightStepSizeType & wStepSize1,
                            const WeightStepSizeType & wStepSize2) {
      return std::get<1>(wStepSize1) < std::get<1>(wStepSize2);
    });

    return std::get<0>(*optimumWeight);
  }

  /*!
   * \brief getUniformlyDistributedPoints
   * \param numberOfPoints
   * \param stepSize
   * \return
   *  Points between [alphaP.alphaD, 1], returns additional one point as it has
   * to add boundaries
   */
  std::vector<double> getUniformlyDistributedPoints(
      const int numberOfPoints, const internal::StepSize& stepSize) const {
    std::vector<double> points(numberOfPoints + 1);
    points[0] = (stepSize.alphaPrimal * stepSize.alphaDual);

    double distance =
        (1 - stepSize.alphaPrimal * stepSize.alphaDual) / numberOfPoints;

    for (int index = 0; index < numberOfPoints; ++index) {
      points[index + 1] = (points[index] + distance);
    }

    return points;
  }

  /*!
   * \brief getBarrierParameter
   * \param currentPoint
   * \param direction
   * \param stepSize
   * \return
   */
  double getBarrierParameter(const internal::Omega& currentPoint) const {
    return ((currentPoint.x.transpose() * currentPoint.s) +
            (currentPoint.tau * currentPoint.kappa)) /
           (currentPoint.x.rows() + 1);
  }

  /*!
   * \brief numberOfCentralityCorrections
   * \return
   *  TODO Incomplete implementation
   */
  int numberOfCentralityCorrections() { return 0; }
};
}

#endif  // LPSOLVER_HPP
